#include "Quartique.h"
#include "stdafx.h"


Quartique::Quartique():Objet(){}

Quartique::Quartique(float* A1, float* B1, float* C1, float* D1, float E1, Point p):A(A1),B(B1),C(C1),D(D1),E(E1),centre(p){}

Quartique::~Quartique(){}

bool Quartique::intersection(Rayon r)
{
	Vecteur directionRayon = r.getDirection();
	Point origineRayon = r.getOrigine();
	float xC = centre.getX(), yC = centre.getY(), zC = centre.getZ();
	float xO = origineRayon.getX()-xC, yO = origineRayon.getY()-yC, zO = origineRayon.getZ()-zC;
	float xD = directionRayon.getX(), yD = directionRayon.getY(), zD = directionRayon.getZ();
	float xO2 = xO*xO, yO2 = yO*yO, zO2 = zO*zO;
	float xD2 = xD*xD, yD2 = yD*yD, zD2 = zD*zD;
	
	float coeffA = A[0]*xD2*xD2 + A[1]*yD2*yD2 + A[2]*zD2*zD2 + A[3]*xD2*xD*yD + A[4]*xD2*xD*zD + A[5]*xD*yD2*yD + A[6]*yD2*yD*zD
				+ A[7]*xD*zD*zD2 + A[8]*yD*zD*zD2 + A[9]*xD2*yD2 + A[10]*xD2*zD2 + A[11]*zD2*yD2 + A[12]*xD2*yD*zD 
				+ A[13]*xD*yD2*zD + A[14]*yD*xD*zD2;

	float coeffB = A[0]*4*xO*xD*xD2 + A[1]*4*yO*yD*yD2 + A[2]*4*zO*zD*zD2 + A[3]*(xD2*xD*yO+3*xD2*xO*yD)
		          + A[4]*(xD2*xD*zO+3*xD2*xO*zD) + A[5]*(yD2*yD*xO+3*yD2*yO*xD) + A[6]*(yD2*yD*zO+3*yD2*yO*zD) 
				  + A[7]*(zD2*zD*xO+3*zD2*zO*xD) + A[8]*(yD2*yD*zO+3*yD2*yO*zD) + A[9]*(2*xD2*yD*yO+2*xO*xD*yD2)
				  + A[10]*(2*xD2*zD*zO+2*xO*xD*zD2) + A[11]*(2*yD2*zD*zO+2*yO*yD*zD2)+ A[12]*(xD2*yD*zO+xD2*yO*zD+2*xD*xO*yD*zD)
				  + A[13]*(yD2*xD*zO+yD2*xO*zD+2*yD*yO*xD*zD)+ A[14]*(zD2*yD*xO+zD2*yO*xD+2*zD*zO*yD*xD)
				  + B[0]*(xD2*xD)+B[1]*(yD2*yD)+B[2]*(zD2*zD)+B[3]*(xD2*yD)+B[4]*(xD2*zD)+ B[5]*(yD2*xD)
				  + B[6]*(yD2*zD)+B[7]*(zD2*xD)+B[8]*(zD2*yD)+B[9]*(xD*yD*zD);
		

	float coeffC = A[0]*(6*xD2*xO2) + A[1]*(6*yD2*yO2) + A[2]*(6*zD2*zO2) + A[3]*(3*xD2*xO*yO+3*xD*xO2*yD)
					+ A[4]*(3*xD2*xO*zO+3*xD*xO2*zD) + A[5]*(3*yD2*yO*xO+3*yD*yO2*xD) + A[6]*(3*yD2*yO*zO+3*yD*yO2*zD)
					+ A[7]*(3*zD2*zO*xO+3*zD*zO2*xD) + A[8]*(3*zD2*zO*yO+3*zD*zO2*yD) + A[9]*(yD2*xO2+xD2*yO2+4*xO*xD*yO*yD)
					+ A[10]*(zD2*xO2+xD2*zO2+4*xO*xD*zO*zD) + A[11]*(yD2*zO2+zD2*yO2+4*zO*zD*yO*yD)
					+ A[12]*(xD2*yO*zO+2*xD*xO*yD*zO+2*xD*xO*yO*zD+xO2*yD*zD)
					+ A[13]*(yD2*xO*zO+2*yD*yO*xD*zO+2*yD*yO*yO*zD+yO2*xD*zD)
					+ A[14]*(zD2*yO*xO+2*zD*zO*xD*yO+2*zD*zO*xO*yD+zO2*xD*yD)
		            + B[0]*3*xD2*xO + B[1]*3*yD2*yO + B[2]*3*zD2*zO+B[3]*(xD2*yO+2*xD*xO*yD)
					+ B[4]*(xD2*zO+2*xD*xO*zD) + B[5]*(yD2*xO+2*yD*yO*xD)+B[6]*(yD2*zO+2*yD*yO*zD)
					+ B[7]*(zD2*xO+2*zD*zO*zD)+B[8]*(zD2*yO+2*zD*zO*yD)+B[9]*(xD*yD*zO+xD*yO*zD+xO*yD*zD)
					+ C[0]*xD2+ C[1]*yD2 + C[2]*zD2 + C[3]*xD*yD + C[4]*xD*zD + C[5]*yD*zD;
	
	float coeffD = A[0]*4*xD*xO2*xO + A[1]*4*yD*yO2*yO + A[2]*4*zD*zO2*zO + A[3]*(yD*xO2*xO+3*xD*xO2*yO)
		           + A[4]*(zD*xO2*xO+3*xD*xO2*zO) + A[5]*(xD*yO2*yO+3*yD*yO2*xO) + A[6]*(zD*yO2*yO+3*yD*yO2*zO)
				   + A[7]*(xD*zO2*zO+3*zD*zO2*xO) + A[8]*(yD*zO2*zO+3*zD*zO2*yO) + A[9]*(2*xD*xO*yO2+2*yD*yO*xO2)
				   + A[10]*(2*xD*xO*zO2+2*zD*zO*xO2) + A[11]*(2*yD*yO*zO2+2*zD*zO*yO2) 
				   + A[12]*(2*xO*xD*yO*zO+xO2*yD*zO+xO2*yO*zD)+ A[13]*(2*yO*yD*xO*zO+yO2*xD*zO+yO2*xO*zD)
				   + A[14]*(2*zO*zD*yO*xO+zO2*yD*xO+zO2*yO*xD)
				   + B[0]*3*xD*xO2 +B[1]*3*yD*yO2 + B[2]*3*zD*zO2
				   + B[3]*(xO2*yD+2*xO*xD*yO) + B[4]*(xO2*zD+2*xO*xD*zO) + B[5]*(yO2*xD+2*yO*yD*xO)
				   + B[6]*(yO2*zD+2*yO*yD*zO) + B[7]*(zO2*xD+2*zO*zD*xO) +B[8]*(zO2*yD+2*zO*zD*yO)
				   + B[9]*(xD*yO*zO+xO*yD*zO+xO*yO*zD)
				   + C[0]*2*xO*xD+ C[1]*2*yO*yD + C[2]*2*zO*zD
				   + C[3]*(xD*yO+xO*yD) + C[4]*(xD*zO+xO*zD) + C[5]*(zD*yO+zO*yD)
				   + D[0]*xD + D[1]*yD + D[2]*zD;

	float coeffE = A[0]*xO2*xO2 + A[1]*yO2*yO2 + A[2]*zO2*zO2
				 + A[3]*xO2*xO*yO+ A[4]*xO2*xO*zO + A[5]*yO2*yO*xO + A[6]*yO2*yO*zO
				 + A[7]*zO2*zO*xO + A[8]*zO2*zO*yO + + A[9]*xO2*yO2 + A[10]*xO2*zO2
				 + A[11]*yO2*zO2 + A[12]*xO2*yO*zO + A[13]*xO*yO2*zO + A[14]*xO*yO*zO2
				 + B[0]*xO2*xO+B[1]*yO2*yO + B[2]*zO2*zO + B[3]*xO2*yO
				 + B[4]*xO2*zO + B[5]*yO2*xO + B[6]*yO2*zO + B[7]*zO2*yO
				 + B[8]*zO2*yO + B[9]*xO2*yO*xO
				 + C[0]*xO2 + C[1]*yO2 +C[2]*zO2+C[3]*xO*yO+C[4]*xO*zO+C[5]*yO*zO
				 +E;
	
    double roots[4];
    int nbRoots = Equation::SolveQuartic(coeffA, coeffB, coeffC, coeffD, coeffE, roots);
	float t=MAXFLOAT;
	for (int i=0 ; i<nbRoots ; i++) {
		if (roots[i] > 0 && roots[i]<t)  {
			t= (float)roots[i];
		}
	}

	if (t==MAXFLOAT || t<0) {
		return false;
	}
	float x = origineRayon.getX()+xD*t, y = origineRayon.getY()+yD*t, z = origineRayon.getZ()+zD*t;
	Point m = Point(x, y, z, 1);

	float xM = xO+xD*t, yM = yO+yD*t, zM = zO+zD*t; 
	float xM2 = xM*xM, yM2 = yM*yM, zM2 = zM*zM, xM3 = xM2*xM, yM3 = yM2*yM, zM3 = zM2*zM;
	inter.setPoint(m);

	float xNormale = A[0]*4*xM3 + A[3]*3*xM2*yM + A[4]*3*xM2*zM + A[5]*yM3 + A[7]*zM3 + A[9]*2*xM*yM2 
					+ A[10]*2*xM*zM2 + A[12]*2*xM*yM*zM + A[13]*yM2*zM + A[14]*zM2*yM 
					+ B[0]*3*xM2 + B[3]*2*xM*yM + B[4]*2*xM*zM + B[5]*yM2 + B[7]*zM2 
					+ B[9]*yM*zM 
					+ C[0]*2*xM + C[3]*yM + C[4]*zM 
					+ D[0];
	float yNormale = A[1]*4*yM3 + A[3]*xM3 + A[5]*3*yM2*xM + A[6]*3*yM2*zM + A[8]*zM3 
					+ A[9]*2*yM*xM2 + A[11]*2*yM*zM2 + A[12]*xM2*zM + A[13]*2*yM*xM*zM + A[14]*zM2*xM 
					+ B[1]*3*yM2 + B[3]*xM2 + B[5]*2*yM*xM + B[6]*2*yM*zM  + B[8]*zM2 + B[9]*xM*zM 
					+ C[1]*2*yM + C[3]*xM + C[5]*zM 
					+ D[1];
	float zNormale = A[2]*4*zM3 + A[4]*xM3 + A[6]*yM3 + A[7]*3*zM2*xM + A[8]*3*zM2*yM  
				 + A[10]*2*zM*xM2 + A[11]*2*zM*yM2 + A[12]*xM2*yM + A[13]*yM2*xM + A[14]*2*zM*xM*yM 
				 + B[2]*3*zM2 + B[4]*xM2 + B[6]*yM2 + B[7]*2*zM*xM + B[8]*2*zM*yM + B[9]*xM*yM 
				 + C[2]*2*zM + C[4]*xM + C[5]*yM 
				 + D[2];
	
	Vecteur normale (xNormale,yNormale,zNormale);
					
	normale.normalize();
	inter.setNormale(normale);
	inter.setT(t);
	return true;

}

BoiteEnglobante Quartique::getBoite()
{
	BoiteEnglobante b;
    
	b.corner1.setX(centre.getX());
	b.corner1.setY(centre.getY());
	b.corner1.setZ(centre.getZ());

	b.corner2.setX(centre.getX());
	b.corner2.setY(centre.getY());
	b.corner2.setZ(centre.getZ());	
    
	return b;
}

bool Quartique::aBoite()
{
	return false;
}